#include "pca9685.h"
#include "tb6612fng.h"
#include "chassis.h"
#include "board.h"

//底盘脱力
void Chassis_deforce(void)
{
    pca_pwmout(STBY_AB, LOW);
    pca_pwmout(STBY_CD, LOW);
}

//底盘上力
void Chassis_onforce(void)
{
    pca_pwmout(STBY_AB, HIGH);
    pca_pwmout(STBY_CD, HIGH);
}

//底盘制动
void Chassis_Stop(void)
{
    pca_pwmout(AIN1, LOW);
    pca_pwmout(AIN2, LOW);
    pca_pwmout(BIN1, LOW);
    pca_pwmout(BIN2, LOW);
    pca_pwmout(CIN1, LOW);
    pca_pwmout(CIN2, LOW);
    pca_pwmout(DIN1, LOW);
    pca_pwmout(DIN2, LOW);
}

/**
 * @brief 	tb6612设定底盘电机转速
 * @note  	-4096<spd[Wheel]<4096
 *  	    传入底盘速度四维向量
 */
void Chassis_SpeedSet(int spd[4])
{
    if(spd[WheelA]>0)
    {
        pca_pwmout(AIN1, HIGH);
        pca_pwmout(AIN2, LOW);
        pca_pwmout(PWMA, 4096-ABS(spd[WheelA]));
    }
    else
    {
        pca_pwmout(AIN1, LOW);
        pca_pwmout(AIN2, HIGH);
        pca_pwmout(PWMA, 4096-ABS(spd[WheelA]));
	}
    if(spd[WheelB]>0)
    {
        pca_pwmout(BIN1, HIGH);
        pca_pwmout(BIN2, LOW);
        pca_pwmout(PWMB, 4096-ABS(spd[WheelB]));
    }
    else
    {
        pca_pwmout(BIN1, LOW);
        pca_pwmout(BIN2, HIGH);
        pca_pwmout(PWMB, 4096-ABS(spd[WheelB]));
    }

    if(spd[WheelC]>0)
    {
        pca_pwmout(AIN1, HIGH);
        pca_pwmout(AIN2, LOW);
        pca_pwmout(PWMA, 4096-ABS(spd[WheelC]));
    }
    else
    {
        pca_pwmout(CIN1, LOW);
        pca_pwmout(CIN2, HIGH);
        pca_pwmout(PWMC, 4096-ABS(spd[WheelC]));
    }

    if(spd[WheelD]>0)
    {
        pca_pwmout(DIN1, HIGH);
        pca_pwmout(DIN2, LOW);
        pca_pwmout(PWMD, 4096-ABS(spd[WheelD]));
    }
    else
    {
        pca_pwmout(DIN1, LOW);
        pca_pwmout(DIN2, HIGH);
        pca_pwmout(PWMD, 4096-ABS(spd[WheelD]));
    }
}

